#include "PoseSolver.h"
#include "Parameter.h"

using namespace cv;
using namespace std;

SendData PoseSolver::resolute(const cv::Mat &depth, const rs2_intrinsics &intrinsics, const cv::Rect &anchor, const float &depScale)
{
    this->_depth = depth;
    this->_depRoi = this->_depth(anchor);
    this->_intrinsics = intrinsics;

    cv::Mat mask;
    // 求解最优平面法向量
    cv::Mat normal = this->_PlaneNormal(this->_depRoi, mask, cameraParam.cameraMatrix);
    // 单位化法向量
    this->_planeNormal = this->_unitNormal(normal);
    // 目标世界坐标系转换
    float *point3D = this->_goalToObject(anchor, depScale);
}

/**
 * @brief 获得色彩渲染的平面掩膜
 * 
 * @return cv::Mat 渲染掩膜
 */
cv::Mat PoseSolver::getColorMask()
{
    cv::Mat colorMask;
    this->_colorMask(this->_mask, colorMask);
    return colorMask;
}

/**
 * @brief  平面分割，从深度源图像中获取平面
 * @param  depth    深度图像
 * @param  mask     
 * @param  Intric   相机内参
 */
cv::Mat PoseSolver::_PlaneNormal(cv::Mat &depth, cv::Mat &mask, cv::Mat &Intric)
{
    cv::rgbd::depthTo3d(depth, Intric, _Mat3D);
    auto rgbdNormals = cv::rgbd::RgbdNormals::create(_Mat3D.cols, _Mat3D.rows, CV_32F, Intric, 1);
    cv::Mat planeNormals;
    // 不进行法向量验证，提升计算速度，损失精度
    this->_rgbdPlane(_Mat3D, mask, planeNormals);
    int index = this->_getMaxPlane(mask);
    this->_mask = mask;
    cv::Mat normal = planeNormals.row(index);
    return normal;
}

/**
 * @brief 单位化平面法向量
 * 
 * @param normal 未单位化法向量
 * @return cv::Mat  单位化法向量
 */
cv::Vec3f PoseSolver::_unitNormal(const cv::Mat &normal)
{
    float a, b, c;
    a = normal.at<float>(0, 0);
    b = normal.at<float>(0, 1);
    c = normal.at<float>(0, 2);
    float A = sqrt(a * a + b * b + c * c);
    cv::Vec3f unitNormals(a / A, b / A, c / A);
    return unitNormals;
}

/**
 * @brief 目标世界坐标系转换
 * 
 * @param goal 目标矩形框
 * @param depScale 深度换算比例
 * @return float* 世界坐标系坐标
 */
float *PoseSolver::_goalToObject(const cv::Rect &goal, const float &depScale)
{
    // 计算anchor中心点
    float pixelPoint[2] = {goal.x + goal.width / 2.f, goal.y + goal.height / 2.f};
    cv::Point center(pixelPoint[0], pixelPoint[1]);
    // 取深度图像素值
    uint16_t depVal = this->_depth.at<uint16_t>(center);
    // 深度比例换算
    float distance = depVal * depScale;
    float point3D[3];
    // 像素坐标系转换为世界坐标系
    rs2_deproject_pixel_to_point(point3D, &this->_intrinsics, pixelPoint, distance);
    return point3D;
}

/**
 * @brief 欧拉角解算
 * 
 * @return cv::Vec3f 
 */
cv::Vec3f PoseSolver::_normalToEuler(const cv::Vec3f &normals)
{
    float yaw, pitch, roll;
}

void PoseSolver::_colorMask(Mat &mask, Mat &colorMask)
{
    RNG rng;
    Vec3b colors[256];
    for (int i = 0; i < 256; i++)
    {
        colors[i] = Vec3b(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
    }

    parallel_for_(Range(0, mask.rows * mask.cols), [&](const Range &range)
                  {
                      for (int r = range.start; r < range.end; r++)
                      {
                          int i = r / mask.cols;
                          int j = r % mask.cols;
                          colorMask.ptr<Vec3b>(i)[j] = colors[mask.ptr<uchar>(i)[j]];
                      }
                  });
}

/**
 * @brief 提取深度源图像中最大的平面
 * @param  mask
 * @return int 
 */
int PoseSolver::_getMaxPlane(Mat &mask)
{
    Mat hist_mat;
    int histSize = 255;
    float range[] = {0, 255};
    const float *histRange = {range};
    bool uniform = true;
    bool accumulate = false;

    calcHist(&mask, 1, 0, Mat(), hist_mat, 1, &histSize, &histRange, uniform, accumulate); //计算直方图，找最大值
    double minVal, maxVal;
    Point minLoc, maxLoc;
    minMaxLoc(hist_mat, &minVal, &maxVal, &minLoc, &maxLoc);

    inRange(mask, Scalar(maxLoc.y), Scalar(maxLoc.y), mask);
    return maxLoc.y;
}

Point PoseSolver::_getPlaneCentroid(Mat &mask)
{
    Mat kerl0 = getStructuringElement(1, Size(11, 11));
    Mat kerl1 = getStructuringElement(1, Size(5, 5));
    // erode(mask, mask, kerl1);
    dilate(mask, mask, kerl0);

    vector<vector<Point>> contours;
    findContours(mask, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

    vector<Point> centeroidPoints;

    for (int i = 0; i < contours.size(); i++)
    {

        cv::Moments centroid = cv::moments(contours[i]);
        cv::Point centeroid(centroid.m10 / centroid.m00, centroid.m01 / centroid.m00);
        if (centeroid.x <= 0 || centeroid.y <= 0)
        {
            continue;
        }
        cout << centeroid << endl;
        centeroidPoints.push_back(centeroid);
    }

    cv::Point centeroid;

    if (centeroidPoints.size() == 1)
    {
        centeroid = centeroidPoints[0];
    }
    else
    {
        cv::Moments centroid = cv::moments(centeroidPoints);
        centeroid = Point(centroid.m10 / centroid.m00, centroid.m01 / centroid.m00);
    }

    return centeroid;
}